#include <AP_gtest.h>

#include <AP_Math/AP_Math.h>

const AP_HAL::HAL& hal = AP_HAL::get_HAL();

const float angle_max_rad = radians(60);
const float angle_limit_rad = radians(60);
float roll_out_rad, pitch_out_rad;

// Test the corners of the input range
TEST(RC2RPTest, Corners) {
    // (-1,-1), (-1,1), (1,-1), (1,1)
    float roll_in[]  = {-1, -1,  1, 1};
    float pitch_in[] = {-1,  1, -1, 1};
    float rc = 37.761f;  // roll at 60 deg max/limit
    float pc = 50.768f;  // pitch at 60 deg max/limit
    float roll_val_deg[]  = {-rc, -rc, rc, rc};
    float pitch_val_deg[] = {-pc, pc, -pc, pc};

    for (uint i=0; i<ARRAY_SIZE(roll_val_deg); i++) {
        rc_input_to_roll_pitch_rad(roll_in[i], pitch_in[i], angle_max_rad, angle_limit_rad, roll_out_rad, pitch_out_rad);

        EXPECT_TRUE(fabsf(roll_out_rad  - radians(roll_val_deg[i]))  < radians(0.001f));
        EXPECT_TRUE(fabsf(pitch_out_rad - radians(pitch_val_deg[i])) < radians(0.001f));
    }
}

// Test some points on the axes
TEST(RC2RPTest, Axes) {
    // (0,-1), (0,1), (1,0), (-1,1)
    float roll_in[]  = { 0,  0,  0, -1,  0.5,  0.0, 1};
    float pitch_in[] = {-1,  1,  0,  0,  0.0,  0.5, 0};
    float roll_val_rad[] = {0,
                            0,
                            0, 
                            -angle_max_rad,
                            angle_max_rad/2,
                            0,
                            angle_max_rad};
    float pitch_val_rad[] = {-angle_max_rad,
                             angle_max_rad, 0,
                             0,
                             0,
                             angle_max_rad/2,
                             0};

    for (uint i=0; i<ARRAY_SIZE(roll_val_rad); i++) {
        rc_input_to_roll_pitch_rad(roll_in[i], pitch_in[i], angle_max_rad, angle_limit_rad, roll_out_rad, pitch_out_rad);

        EXPECT_TRUE(fabsf(roll_out_rad  - roll_val_rad[i])  < radians(0.001f));
        EXPECT_TRUE(fabsf(pitch_out_rad - pitch_val_rad[i]) < radians(0.001f));
    }

}

// Test some points on the circle at 60 degrees
TEST(RC2RPTest, Circle) {

    // values generated by VPython implementation; roll/pitch deltas are < .02 degrees w.r.t C++ code
    float xy_rp[][4] = {
        {1.000, 0.000, 60.000, 0.000},
        {0.924, 0.383, 53.193, 22.961},
        {0.707, 0.707, 34.005, 42.426},
        {0.383, 0.924, 13.516, 55.433},
        {0.000, 1.000, 0.000, 60.000},
        {-0.383, 0.924, -13.516, 55.433},
        {-0.707, 0.707, -34.005, 42.426},
        {-0.924, 0.383, -53.193, 22.961},
        {-1.000, 0.000, -60.000, 0.000},
        {-0.924, -0.383, -53.193, -22.961},
        {-0.707, -0.707, -34.005, -42.426},
        {-0.383, -0.924, -13.516, -55.433},
        {-0.000, -1.000, -0.000, -60.000},
        {0.383, -0.924, 13.516, -55.433},
        {0.707, -0.707, 34.005, -42.426},
        {0.924, -0.383, 53.193, -22.961}
    };

    for (uint row=0; row<ARRAY_SIZE(xy_rp); row++) {
        float roll_in = xy_rp[row][0];
        float pitch_in = xy_rp[row][1];
        rc_input_to_roll_pitch_rad(roll_in, pitch_in, angle_max_rad, angle_limit_rad, roll_out_rad, pitch_out_rad);

        EXPECT_TRUE(fabsf(roll_out_rad  - radians(xy_rp[row][2])) < radians(0.02f));
        EXPECT_TRUE(fabsf(pitch_out_rad - radians(xy_rp[row][3])) < radians(0.02f));
    }
}


AP_GTEST_MAIN();
